from machine import Pin
import sensor, image
import  pyb
from pyb import UART, LED,Pin, Timer
import  ustruct
import time
# 初始化激光灯控制引脚，并置为高电平
laser_light=Pin("C9", Pin.OUT)
laser_light.value(1)
red_td1 =[(42, 100, 12, 74, -41, 64)]
red_td =[(13, 87, 24, 45, -30, 24)]
#green_td =[(43, 98, -78, -19, -51, 21)]
green_td1 =[(64, 98, -37, 12, -23, 18)]
green_td2 =[(66, 100, -40, -4, -79, 28)]
green_td =[(58, 100, -67, -15, 2, 50)   ]
# 初始化TFT180屏幕
roi1=[(50,19,227,181)]
uart = UART(3, 115200) # todo: 几号口不确定
flag_test=1
def sending_data(cx1,cy1):
    global uart;
    cx1=cx1+320
    cy1=cy1+240
    data = ustruct.pack("<bHHb",      #格式为俩个字符俩个短整型(2字节)
                   0x86,                      #帧头1
                   cx1,   # up sample by 4   #数据1
                   cy1,   # up sam9ple by 4    #数据2
                   0x54)
    uart.write(data);

# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565) # 设置图像色彩格式为RGB565格式
sensor.set_framesize(sensor.QVGA)  # 设置图像大小为160*120
sensor.set_auto_whitebal(False)      # 设置自动白平衡
sensor.set_auto_gain(False)

sensor.set_brightness(3500)         # 设置亮度为3000
sensor.skip_frames(time = 20)       # 跳过帧

clock = time.clock()
start_flag=0
start_send=0
x_last=0
y_last=0
while(True):
    clock.tick()
    img = sensor.snapshot()

    if uart.any():
        cs=uart.readchar()
        if cs==56:
            if start_send==1:
                start_send=0
            else :
                start_send=1
        print("cs:",cs)
    red_blods =  img.find_blobs(red_td,pixels_threshold=1, area_threshold=2, merge=True,invert = 0,roi=(39,36,238,156))
    green_blods = img.find_blobs(green_td,pixels_threshold=1, area_threshold=2, merge=True,invert = 0,roi=(39,36,238,156))
    if start_flag==0:
        if green_blods:
            start_flag=1
            green_blod=green_blods[0]
    if start_flag==1:
        if red_blods:

            red_blod = max(red_blods, key=lambda x: x.area())
            print("red",red_blod)
            if flag_test==1:
                img.draw_circle((red_blod.cx(), red_blod.cy(),int((red_blod.w()+red_blod.h())/4)))
                img.draw_rectangle(red_blod.rect(), color = (0, 0, 255), scale = 2, thickness = 2)
        if green_blods:
            green_blod = max(green_blods, key=lambda x: x.area())
            print("green",green_blod)
            #if green_blod.area()>150:
                #for blob in green_blods[:]:
                    #if blob.area() == green_blod.area():
                        #green_blods.remove(blob)
                        #green_blod = max(green_blods, key=lambda x: x.area())
                        #break
            if flag_test==1:
                img.draw_circle((green_blod.cx(), green_blod.cy(),int((green_blod.w()+green_blod.h())/4)))
                img.draw_rectangle(green_blod.rect(), color = (255, 0, 255), scale = 2, thickness = 2)
        if green_blods and red_blods:
            dis_x=(green_blod.x() + green_blod.w()/2)-red_blod.cx()
            dis_y=green_blod.cy()-red_blod.cy()
            if start_send==1:
                sending_data( int(dis_x) ,int (dis_y))
                x_last=dis_x
                y_last=dis_y
                print(x_last,y_last)
        else :
            print (x_last,y_last)
            #sending_data( int(x_last) ,int (y_last))
            #print(dis_x,dis_y)
        #else :
            #print("none")
            #sending_data(0,0)
    # 打印帧率
    #print(clock.fps())
